#ifndef BASE_DRIVER_H_
#define BASE_DRIVER_H_

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <iostream>
#include <serial/serial.h> //ROS的串口包 http://wjwwood.io/serial/doc/1.1.0/index.html
#include <math.h>
#include <fstream>
#include <boost/thread.hpp>
#include <string>
#include <ros/package.h>

#include <string>
#include <vector>
#include <math.h>

#include <boost/asio.hpp>
#include <boost/asio/serial_port.hpp>
#include <boost/system/error_code.hpp>
#include <boost/system/system_error.hpp>
#include <boost/bind.hpp>
#include <boost/thread.hpp>

using namespace std;


namespace CAR_CONTROL
{

#define head 0xA6

enum packetFinderState
{
    waitingForHead,
    waitingForPayloadSize,
    waitingForPayloadType,
    waitingForPayload,
    waitingForCheckSum,
    handlePayload
};

enum faultSymbolState
{
    receiveTimeout
   
};

enum modeHandler
{
   startMode,
   stopMode,
   runningMode,
   transitionMode,
   timingMode,
   error_default


};

// typedef boost::shared_ptr<boost::asio::serial_port> serialp_ptr;
class carControl
{
public:
  carControl();
  ~carControl();
  void processLoop();
  ros::NodeHandle nh_;

private:
  bool if_debug_;
  //sum info
  int sn_lost_ = 0;
  int crc_error_ = 0;
  uint8_t read_sn_ = 0;
  bool frist_sn_;
  int device_type_ = 1;
  packetFinderState state_;
  //serial
  serial::Serial serial_; //声明串口对象
  std::string serial_port_;
  int serial_baud_;
  int serial_timeout_;
  //time
  ros::Time last_time_;
  ros::Time now_;
  ros::Time last_twist_time_;
  ros::Time running_count;

  //string
  string car_topic_;
  string base_frame_id_;

  //boost
  boost::mutex cmd_vel_mutex_;
  boost::mutex mutex_;
  //flag
  int faultSymbol_flag;
  uint8_t x_last;
  uint8_t z_last;
  uint8_t car_mode;

  //frame name
  string imu_frame_id_;

  //topic
  string imu_topic_, mag_pose_2d_topic_;
  
  //Publisher
  ros::Publisher car_pub_;
  ros::Publisher mag_pose_pub_;
  ros::Subscriber cmd_sub_;
  //data
  geometry_msgs::Twist current_twist_;
  double linear_speed, angular_speed;
  int linear,angular;

  int write_faultSymbolState(faultSymbolState bit);
  void cmd_data_processing(double& x,double& z,uint8_t& mode);
  void Pattern_handler(double& x,double& z);
  void check_sum(uint8_t* data, size_t len, uint8_t& dest);
  void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg);
  void send_speed_callback(const ros::TimerEvent&);
  void receive_func();


}; //carControl
} // namespace CAR_CONTROL

#endif